import numpy as np
import cv2
import open3d as o3d
from Camera import Camera
import numpy as np
import cv2
import open3d as o3d
from Camera import Camera

class VolumeCalculator:
    """
    物料体积计算类，用于计算不同形状物料的体积
    """

    def __init__(self,point,height_D,disparity,img_color):

        self.point = point
        self.height_D = height_D
        self.depth_map = disparity  # 视差信息
        self.img_color = img_color
        size = (640, 480)
        file_path = "./相机信息/out.xls"
        self.camera = Camera(file_path, size)

        self.height, self.width = self.depth_map.shape
        pass



    """
    计算体积 方法1 真实面积乘以深度距离框的深度
    缺点是 :积分是以长方体的形式，实际上不是长方体
    """
    def calculate_Bulk_Volume_points(self, height, rotation_matrix):
        """ 计算红色点云相对于旋转平面的体积（面积 × 距离 累加） """

        # 计算旋转后的法向量（Z 轴）
        normal_vector = rotation_matrix @ np.array([0, 0, 1])
        a, b, c = normal_vector  # 法向量

        # 计算旋转平面上的一点
        reference_point = rotation_matrix @ np.array([0, 0, height])  # 目标平面 height
        d = -np.dot([a, b, c], reference_point)  # 平面方程常数项 d

        total_volume = 0  # 最终积分体积

        # 遍历点云，跳过边界以防越界
        for v in range(self.height - 1):
            for u in range(self.width - 1):
                D = self.depth_map[v, u]
                if D == 0 or D < 40:
                    continue
                Z = (self.camera.b * self.camera.f_x) / D
                if Z <= 0:
                    continue

                # 当前点坐标
                X = self.camera.b * (u - self.camera.cx) / D
                Y = self.camera.b * (v - self.camera.cy) / D



                # 相邻右边点
                D_right = self.depth_map[v, u + 1]
                Z_right = (self.camera.b * self.camera.f_x) / D_right if D_right > 40 else Z
                X_right = self.camera.b * (u + 1 - self.camera.cx) / D_right if D_right > 40 else X
                Y_right = self.camera.b * (v - self.camera.cy) / D_right if D_right > 40 else Y

                # 相邻下方点
                D_down = self.depth_map[v + 1, u]
                Z_down = (self.camera.b * self.camera.f_x) / D_down if D_down > 40 else Z
                X_down = self.camera.b * (u - self.camera.cx) / D_down if D_down > 40 else X
                Y_down = self.camera.b * (v + 1 - self.camera.cy) / D_down if D_down > 40 else Y

                # 计算两条边
                vec_right = np.array([X_right - X, Y_right - Y, Z_right - Z])
                vec_down = np.array([X_down - X, Y_down - Y, Z_down - Z])

                # 叉乘得到面积
                cross_product = np.cross(vec_right, vec_down)
                area = 0.5 * np.linalg.norm(cross_product)  # 平行四边形面积一半，即三角形面积

                # 点到旋转平面的距离
                distance = abs(a * X + b * Y + c * Z + d) / np.sqrt(a ** 2 + b ** 2 + c ** 2)

                # 面积 × 距离 累加作为体积元素
                total_volume += area * distance
        print(f"堆料体积为{total_volume}m³。")
        #return total_volume
    #todo 写一个计算骨料仓余量的方法:用堆料仓总体积-堆料体积；堆料仓长宽高的参数由BoneStorageModel的类传入